// This is the Arduino firmware for Colbot.
// Includes:
// - Pan/Tilt head servos simple command protocol

#include <Servo.h> 
 
Servo myservoPan;  
Servo myservoTilt;

// Magic number starting packet
const int START_TAG='S';
// Pan='P'
const int PAN_CMD='P';
// TILT='T'
const int TILT_CMD='T';

// the number of the LED pin:
const int ledPin =  13;      

// mecanical offset of pan servo (i.e. value to add to 90 deg to be at real 90)
const int panOffset=-20;

// mecanical offset of tilt servo (i.e. value to add to 90 deg to be at real 90)
const int tiltOffset=-15;
 
void setup() 
{ 
  Serial.begin(9600);
  myservoPan.attach(7); 
  myservoTilt.attach(6); 
  
  myservoPan.write(90+panOffset); 
  myservoTilt.write(90+tiltOffset); 
  
  pinMode(ledPin, OUTPUT);    
  digitalWrite(ledPin, HIGH);
  delay(500);
  digitalWrite(ledPin, LOW);
} 
 
 
void loop() 
{ 
    // wait start tag (resync):
    while(Serial.read()!=START_TAG);
    // now need 2 bytes more:
    while(Serial.available() < 2) delay(1);
    // read command and act:    
    int cmd=Serial.read();
    if (cmd==PAN_CMD) {
      int pos=Serial.read();
      myservoPan.write(pos+panOffset); 
      Serial.print('P', BYTE); // ack
    } else if (cmd==TILT_CMD) {
      int pos=Serial.read();
      myservoTilt.write(pos+tiltOffset);
      Serial.print('T', BYTE);  // ack
    } else {
      digitalWrite(ledPin, HIGH);
      delay(500);
      digitalWrite(ledPin, LOW);
      Serial.print('?', BYTE);  // nack
    }
} 
